/*
 * CoreWrapper.cpp
 *
 *  Created on: 2 févr. 2010
 *      Author: labm2414
 */

#include "CoreWrapper.h"
#include "MsgConversion.h"
#include <ros/ros.h>
#include <semol/core/SemolEvent.h>
#include <semol/core/Parameters.h>
#include <semol/core/SensorimotorEvent.h>
#include <semol/core/ParamEvent.h>
#include <utilite/UEventsManager.h>
#include <utilite/ULogger.h>
#include <utilite/UFile.h>
#include <utilite/UStl.h>
#include <opencv2/highgui/highgui.hpp>
#include <utilite/UImageEvent.h>

//msgs
#include "semol/Info.h"
#include "semol/InfoEx.h"
#include "semol/CvMatMsg.h"

using namespace semol;

CoreWrapper::CoreWrapper(bool deleteDbOnStart)
{
	ros::NodeHandle nh("~");
	infoPub_ = nh.advertise<semol::Info>("info", 1);
	infoPubEx_ = nh.advertise<semol::InfoEx>("infoEx", 1);

	ParametersMap parameters = loadParameters(semol_.getIniFilePath());

	for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
	{
		nh.setParam(iter->first, iter->second);
	}

	semol_.init(parameters, deleteDbOnStart);

	resetMemorySrv_ = nh.advertiseService("resetMemory", &CoreWrapper::resetMemoryCallback, this);
	dumpMemorySrv_ = nh.advertiseService("dumpMemory", &CoreWrapper::dumpMemoryCallback, this);
	deleteMemorySrv_ = nh.advertiseService("deleteMemory", &CoreWrapper::deleteMemoryCallback, this);
	dumpPredictionSrv_ = nh.advertiseService("dumpPrediction", &CoreWrapper::dumpPredictionCallback, this);

	nh = ros::NodeHandle();
	parametersUpdatedTopic_ = nh.subscribe("semol_gui/parameters_updated", 1, &CoreWrapper::parametersUpdatedCallback, this);
	sensorimotorTopic_ = nh.subscribe("sensorimotor", 1, &CoreWrapper::sensorimotorReceivedCallback, this);
}

CoreWrapper::~CoreWrapper()
{
	this->saveParameters(semol_.getIniFilePath());
	semol_.close();
}

ParametersMap CoreWrapper::loadParameters(const std::string & configFile)
{
	ParametersMap parameters = Parameters::getDefaultParameters();
	if(!configFile.empty())
	{
		ROS_INFO("Loading parameters from %s", configFile.c_str());
		if(!UFile::exists(configFile.c_str()))
		{
			ROS_WARN("Config file doesn't exist! It will be generated...");
		}
		Semol::readParameters(configFile.c_str(), parameters);
	}
	// otherwise take default parameters

	return parameters;
}

void CoreWrapper::saveParameters(const std::string & configFile)
{
	if(!configFile.empty())
	{
		printf("Saving parameters to %s\n", configFile.c_str());

		if(!UFile::exists(configFile.c_str()))
		{
			printf("Config file doesn't exist, a new one will be created.\n");
		}

		ParametersMap parameters = Parameters::getDefaultParameters();
		ros::NodeHandle nh("~");
		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
		{
			std::string value;
			if(nh.getParam(iter->first,value))
			{
				iter->second = value;
				printf("Param %s = %s\n", iter->first.c_str(), value.c_str());
			}
			else
			{
				printf("cannot read rosparam %s\n", iter->first.c_str());
			}
		}

		Semol::writeParameters(configFile.c_str(), parameters);
	}
	else
	{
		ROS_INFO("Parameters are not saved! (No configuration file provided...)");
	}
}

void CoreWrapper::sensorimotorReceivedCallback(const semol::SensorimotorConstPtr & msg)
{
	std::list<SensorRaw> sensors;
	std::list<Actuator> actuators;

	for(unsigned int i=0; i<msg->sensors.size(); ++i)
	{
		sensors.push_back(SensorRaw((Sensor::Type)msg->sensors[i].type, fromCvMatMsgToCvMat(msg->sensors[i].matrix)));
	}
	for(unsigned int i=0; i<msg->actuators.size(); ++i)
	{
		actuators.push_back(Actuator(fromCvMatMsgToCvMat(msg->actuators[i].matrix), (Actuator::Type)msg->actuators[i].type));
	}

	if(!sensors.size() && !actuators.size())
	{
		ROS_ERROR("Sensorimotor received is empty...");
	}
	else
	{
		ROS_INFO("Received sensorimotor (%d sensors %d actuators).", (int)sensors.size(), (int)actuators.size());
		semol_.process(sensors, actuators);

		const Statistics & stats = semol_.getStatistics();
		this->publishStatistics(stats);
	}
}

bool CoreWrapper::resetMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
	semol_.resetMemory();
	return true;
}

bool CoreWrapper::dumpMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
	semol_.dumpData();
	return true;
}

bool CoreWrapper::deleteMemoryCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
	semol_.resetMemory(true);
	return true;
}

bool CoreWrapper::dumpPredictionCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
	semol_.dumpPrediction();
	return true;
}

void CoreWrapper::parametersUpdatedCallback(const std_msgs::EmptyConstPtr & msg)
{
	semol::ParametersMap parameters = semol::Parameters::getDefaultParameters();
	ros::NodeHandle nh("~");
	for(semol::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
	{
		std::string value;
		if(nh.getParam(iter->first, value))
		{
			iter->second = value;
		}
	}
	ROS_INFO("Updating parameters");
	semol_.parseParameters(parameters);
}

void CoreWrapper::publishStatistics(const Statistics & statistics)
{
	if(infoPub_.getNumSubscribers() || infoPubEx_.getNumSubscribers())
	{
		const Statistics & stat = statistics;

		ROS_INFO("info=%d infoEx=%d", infoPub_.getNumSubscribers(), infoPubEx_.getNumSubscribers());

		// General info
		if(infoPub_.getNumSubscribers())
		{

			semol::InfoPtr msg(new semol::Info);
			msg->refId = stat.refImageId();
			msg->loopClosureId = stat.loopClosureId();
			msg->actuators.resize(stat.getActuators().size());
			int i=0;
			for(std::list<Actuator>::const_iterator iter = stat.getActuators().begin(); iter!=stat.getActuators().end(); ++iter)
			{
				msg->actuators[i].type = iter->type();
				fromCvMatToCvMatMsg(msg->actuators[i++].matrix, iter->data(), false);
			}
			ROS_INFO("Sending Info msg (last_id=%d)...", stat.refImageId());
			infoPub_.publish(msg);
		}

		if(infoPubEx_.getNumSubscribers())
		{
			semol::InfoExPtr msg(new semol::InfoEx);

			msg->refId = stat.refImageId();
			msg->loopClosureId = stat.loopClosureId();
			msg->actuators.resize(stat.getActuators().size());
			int i=0;
			for(std::list<Actuator>::const_iterator iter = stat.getActuators().begin(); iter!=stat.getActuators().end(); ++iter)
			{
				msg->actuators[i].type = iter->type();
				fromCvMatToCvMatMsg(msg->actuators[i++].matrix, iter->data(), false);
			}

			// Detailed info
			if(stat.extended())
			{
				// sensor raw data
				if(stat.refRawData().size())
				{
					msg->refRawData.resize(stat.refRawData().size());
					i=0;
					for(std::list<SensorRaw>::const_iterator iter = stat.refRawData().begin(); iter!=stat.refRawData().end(); ++iter)
					{
						msg->refRawData[i].type = iter->type();
						fromCvMatToCvMatMsg(msg->refRawData[i++].matrix, iter->data(), true);
					}
				}
				if(stat.loopClosureRawData().size())
				{
					msg->loopRawData.resize(stat.loopClosureRawData().size());
					i=0;
					for(std::list<SensorRaw>::const_iterator iter = stat.loopClosureRawData().begin(); iter!=stat.loopClosureRawData().end(); ++iter)
					{
						msg->loopRawData[i].type = iter->type();
						fromCvMatToCvMatMsg(msg->loopRawData[i++].matrix, iter->data(), true);
					}
				}

				// sensor post data
				ROS_INFO("stat.refSensorData().size()=%d", (int)stat.refSensorData().size());
				if(stat.refSensorData().size())
				{
					msg->refSensorData.resize(stat.refSensorData().size());
					i=0;
					for(std::list<std::vector<float> >::const_iterator iter = stat.refSensorData().begin(); iter!=stat.refSensorData().end(); ++iter)
					{
						cv::Mat m(1, iter->size(), CV_32FC1);
						memcpy(m.data, iter->data(), iter->size()*sizeof(float));
						fromCvMatToCvMatMsg(msg->refSensorData[i++], m, false);
					}
				}
				if(stat.loopSensorData().size())
				{
					msg->loopSensorData.resize(stat.loopSensorData().size());
					i=0;
					for(std::list<std::vector<float> >::const_iterator iter = stat.loopSensorData().begin(); iter!=stat.loopSensorData().end(); ++iter)
					{
						cv::Mat m(1, iter->size(), CV_32FC1);
						memcpy(m.data, iter->data(), iter->size()*sizeof(float));
						fromCvMatToCvMatMsg(msg->loopSensorData[i++], m, false);
					}
				}

				//Posterior, likelihood, childCount
				msg->posteriorKeys = uKeys(stat.posterior());
				msg->posteriorValues = uValues(stat.posterior());
				msg->likelihoodKeys = uKeys(stat.likelihood());
				msg->likelihoodValues = uValues(stat.likelihood());
				msg->weightsKeys = uKeys(stat.weights());
				msg->weightsValues = uValues(stat.weights());

				// Statistics data
				msg->statsKeys = uKeys(stat.data());
				msg->statsValues = uValues(stat.data());

			}
			ROS_INFO("Sending InfoEx msg (last_id=%d)...", stat.refImageId());
			infoPubEx_.publish(msg);
		}
	}
}
